#include "detectorwindow.h"

using namespace std;

DetectorWindow::DetectorWindow(QWidget *parent) :
    QWidget(parent)
{
    qRegisterMetaType<Point>( "Point" );
    qRegisterMetaType<Point2f>( "Point2f" );

    capture0 = cvCreateCameraCapture( 0 );
    capture1 = cvCreateCameraCapture( 1 );

    _pv_0 = new PreviewWindow( this );
    _pv_1 = new PreviewWindow( this );

    _layout_main = new QGridLayout( this );
    _layout_main->addWidget( new QLabel( tr("Camera view #0") ), 0, 0 );
    _layout_main->addWidget( new QLabel( tr("Camera view #1") ), 0, 1 );
    _layout_main->addWidget( _pv_0, 1, 0 );
    _layout_main->addWidget( _pv_1, 1, 1 );

    _layout_main->setContentsMargins( 1, 1, 1, 1 );

    setLayout( _layout_main );
    setFixedSize( 970, 740 );

    loadCamParams();
    pnts3d = Mat( 2, 1, CV_32FC3, Scalar::all( 0 ) );

    _detector0 = new BlinkDetector( capture0, this );
    _detector1 = new BlinkDetector( capture1, this );
    connect( _detector0, SIGNAL(imageChanged(IplImage*,IplImage*)),
             _pv_0, SLOT(displayCVImage(IplImage*,IplImage*)));
    connect( _detector1, SIGNAL(imageChanged(IplImage*,IplImage*)),
             _pv_1, SLOT(displayCVImage(IplImage*,IplImage*)));

    connect( _detector0, SIGNAL(povChanged(Point2f,Point2f)),
             this, SLOT(updateEyeCam0(Point2f,Point2f)) );

    connect( _detector1, SIGNAL(povChanged(Point2f,Point2f)),
             this, SLOT(updateEyeCam1(Point2f,Point2f)) );
    _detector0->start();
    _detector1->start();
}


void DetectorWindow::loadCamParams()
{
    /* Read in pre-calibrated stereo camera configurations */
    FileStorage fs_in( "intrinsics.yml", FileStorage::READ );
    FileStorage fs_ex( "extrinsics.yml", FileStorage::READ );

    //Rect validRoi[2];
    fs_in["M1"] >> cameraMatrix[0];
    fs_in["M2"] >> cameraMatrix[1];
    fs_in["D1"] >> distCoeffs[0];
    fs_in["D2"] >> distCoeffs[1];

    fs_ex["R1"] >> R[0];
    fs_ex["R2"] >> R[1];
    fs_ex["P1"] >> P[0];
    fs_ex["P2"] >> P[1];
    fs_ex["Q"]  >> Q;
}

void DetectorWindow::updateEyeCam0 ( Point2f leye, Point2f reye )
{
    if( leye.x < 0 || reye.x < 0 )
    {
        leyes[0] = leye;
        reyes[0] = reye;
    }
    else
    {
        vector<Point2f> pnts;
        pnts.push_back( leye );
        pnts.push_back( reye );

        undistortPoints( pnts, pnts, cameraMatrix[0], distCoeffs[0], R[0], P[0] );

        leyes[0] = pnts.at(0);
        reyes[0] = pnts.at(1);

        if( leyes[1].x > 0 && reyes[1].x > 0 )
        {
            pnts3d.row( 0 ) = Scalar( leyes[0].x, leyes[0].y, abs( leyes[0].x - leyes[1].x ) ) ;
            pnts3d.row( 1 ) = Scalar( reyes[0].x, reyes[0].y, abs( reyes[0].x - reyes[1].x ) ) ;

            perspectiveTransform( pnts3d, pnts3d, Q );
            pnts3d = pnts3d * UNIT_SIZE;
        }
    }
    _pv_0->updateEyeCam( leyes[0], reyes[0], pnts3d );
}

void DetectorWindow::updateEyeCam1 ( Point2f leye, Point2f reye )
{
    if( leye.x < 0 || reye.x < 0 )
    {
        leyes[1] = leye;
        reyes[1] = reye;
    }
    else
    {
        vector<Point2f> pnts;
        pnts.push_back( leye );
        pnts.push_back( reye );

        undistortPoints( pnts, pnts, cameraMatrix[1], distCoeffs[1], R[1], P[1] );

        leyes[1] = pnts.at(0);
        reyes[1] = pnts.at(1);

        if( leyes[0].x > 0 && reyes[0].x > 0 )
        {
            pnts3d.row( 0 ) = Scalar( leyes[0].x, leyes[0].y, abs( leyes[0].x - leyes[1].x ) ) ;
            pnts3d.row( 1 ) = Scalar( reyes[0].x, reyes[0].y, abs( reyes[0].x - reyes[1].x ) ) ;

            perspectiveTransform( pnts3d, pnts3d, Q );
            pnts3d = pnts3d * UNIT_SIZE;
        }
    }
    _pv_1->updateEyeCam( leyes[1], reyes[1], pnts3d );
}

QSize  PreviewWindow::PV_SIZE =  QSize( PV_W,PV_H );
QImage PreviewWindow::dummy   = QImage( PV_SIZE, QImage::Format_RGB32 );

PreviewWindow::PreviewWindow( QWidget *parent )
    :QWidget( parent )
{
    _frame = new QLabel;
    _km    = new QLabel;

    QLayout * layout = new QVBoxLayout( this );
    layout->setContentsMargins( 1, 1, 1, 1 );

    QPixmap temp = QPixmap::fromImage( dummy );
    _frame->setPixmap( temp );
    _km->setPixmap( temp );

    layout->addWidget( _frame );
    layout->addWidget( _km );

    for (int x = 0; x < PV_W; x ++) {
        for (int y =0; y < PV_H; y++) {
            dummy.setPixel(x,y,qRgb(x/PV_W*255, y/PV_H*255, y/PV_W*255));
        }
    }
}

void PreviewWindow::updateEyeCam( Point2f left, Point2f right, Mat pnts3d )
{
    QPixmap kmap = QPixmap( PV_SIZE );
    QPainter painter( &kmap );
    painter.setPen( QColor( Qt::white ) );
    QFont font( "Consolas" );
    painter.setFont( font );
    QString out;
    out = "Left  eye :> (" + QString::number( left.x, 'f', 1 )
            + ", " + QString::number( left.y, 'f', 1 ) + ")";
    painter.drawText( 30, 30, out );
    out = "Right eye :> (" + QString::number( right.x, 'f', 1  )
            + ", " + QString::number( right.y, 'f', 1  ) + ")";
    painter.drawText( 30, 60, out );

    QString sp[2];
    for( int i = 0; i < 2; i ++ )
    {
        Vec3f t_row = pnts3d.at<Vec3f>( i, 0 );
        sp[i] = "(" + QString::number( t_row[0], 'f', 1 );
        sp[i] += ", " + QString::number( t_row[1], 'f', 1 );
        sp[i] += ", " + QString::number( t_row[2], 'f', 1 ) + ")";
    }
    painter.setPen( QColor( Qt::yellow ) );
    out = "3D Left  eye :>" + sp[0];
    painter.drawText( 30, 90, out );
    out = "3D Right eye :>" + sp[1];
    painter.drawText( 30, 120, out );
    _km->setPixmap( kmap );
}

QPixmap PreviewWindow::cvtCvImg2QtImg( IplImage * ipl )
{
    if( ipl->depth != IPL_DEPTH_8U || ipl->nChannels != 3 )
    {
        qDebug() << "format not supported";
        return QPixmap::fromImage( dummy );
    }
    QImage temp(ipl->width, ipl->height, QImage::Format_RGB32);
    int cvIndex = 0; int cvLineStart = 0;
    for (int y = 0; y < ipl->height; y++)
    {
        unsigned char red,green,blue;
        cvIndex = cvLineStart;
        for (int x = 0; x < ipl->width; x++)
        {
            red = ipl->imageData[cvIndex+2];
            green = ipl->imageData[cvIndex+1];
            blue = ipl->imageData[cvIndex+0];

            temp.setPixel(x,y,qRgb(red, green, blue));
            cvIndex += 3;
        }
        cvLineStart += ipl->widthStep;
    }
    return QPixmap::fromImage( temp );
}

void PreviewWindow::displayCVImage( IplImage *frame, IplImage *kmimg )
{
    QPixmap f = cvtCvImg2QtImg( frame ), k;

    f = f.scaled( PV_SIZE, Qt::IgnoreAspectRatio, Qt::SmoothTransformation );
    _frame->setPixmap( f );

    if( kmimg )
    {
        k = cvtCvImg2QtImg( kmimg );
        k = k.scaled( PV_SIZE  );
        _km->setPixmap( k );
    }

}

